#ifndef __CURVEFITTING__
#define __CURVEFITTING__

#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Sparse>
#include "Function.h"
#include "Polynomial.h"
#include "Interpolation.h"
#include "spline.h"

using namespace std;
using namespace Eigen;
class Curve_fitting : public Interpolation
{
 private:
  vector<vector<double>> Point, Curve;
  vector<double> cumulative_chordal_length;
  int n, dim, spline_method, order;
  double interval;
public:
  Curve_fitting(
		vector<vector<double>> _Point,
		int _spline_method,
		int _order){
    Point = _Point;
    spline_method = _spline_method;
    order = _order;
    if ((spline_method != 1) && (spline_method != 2))
      {
	cout<< "error: There is no such spline method." <<endl;
	  exit(0);
      }
    if ((order != 1) && (order != 3))
      {
	cout<< "error: There is no such order." <<endl;
	  exit(0);
      }
    n = Point.size();
    dim = Point[0].size();
  }

  void solve()
  {
    interval = 0.01;
    cumulative_chordal_length.push_back(0.0);
    for (int i = 1; i < n; i++)
      {
	cumulative_chordal_length.push_back(cumulative_chordal_length[i-1] + norm_2(Point[i],Point[i-1]));
      }
    vector<double> z(n);
    double length;
    for (int i = 0; i < dim; i++)
      {
	vector<double> p, fitp;
	for (int j = 0; j < n; j++)
	  {
	    p.push_back(Point[j][i]);
	  }
	Input f(cumulative_chordal_length, p, z, z);
	if (spline_method == 1)
	  {
	    ppForm_interpolation pp(f, cumulative_chordal_length, 3,order);
	    pp.solve();
	    length = 0;
	    while (length <= cumulative_chordal_length[n-1])
	      {
		fitp.push_back(pp(length));
		length = length + interval;
	      }
	  }
	else
	  {
	    Bspline_interpolation pp(f, cumulative_chordal_length, 3,order);
	    pp.solve();
	    length = 0;
	    while (length <= cumulative_chordal_length[n-1])
	      {
		fitp.push_back(pp(length));
		length = length + interval;
	      }
	  }
	Curve.push_back(fitp);
      }
  }

    vector<vector<double> > Get_point()
    {
      return Curve;
    }
  
  double norm_2(vector<double> x1, vector<double> x2)
  {
    double dis = 0.0;
    for (int i = 0;i < dim; i++)
      {
	dis = dis + (x1[i] - x2[i]) * (x1[i] - x2[i]);
      }
    dis = sqrt(dis);
    return dis;
  }
};
#endif
